#include "visualosc.h"
#include "export_c.h"

uint8_t VisualOsc::m_buffer[NIMING_TEMP_BUFFER_SIZE];
int VisualOsc::m_bufferLen = 0;
bool VisualOsc::m_enabled = true;

void VisualOsc::SetEnabled(bool enabled)
{
    m_enabled = enabled;
}

/**
 * @brief       发送状态帧至匿名地面站V4
 * @param       rol     : 横滚角
 *              pit     : 俯仰角
 *              yaw     : 航向角
 *              alt     : 飞行高度，单位：cm
 *              fly_mode: 飞行模式
 *              armed   : 锁定状态，0xA0：加锁 0xA1：解锁
 * @retval      无
 */
void VisualOsc::NimingOldReportIMUAngle(int16_t rol, int16_t pit, int16_t yaw, uint32_t alt, uint8_t fly_mode, uint8_t armed)
{
    uint8_t send_buf[12];
    
    /* 横滚角 */
    send_buf[0] = (rol >> 8) & 0xFF;
    send_buf[1] = rol & 0xFF;
    /* 俯仰角 */
    send_buf[2] = (pit >> 8) & 0xFF;
    send_buf[3] = pit & 0xFF;
    /* 航向角 */
    send_buf[4] = (yaw >> 8) & 0xFF;
    send_buf[5] = yaw & 0xFF;
    /* 飞行高度 */
    send_buf[6] = (alt >> 24) & 0xFF;
    send_buf[7] = (alt >> 16) & 0xFF;
    send_buf[8] = (alt >> 8) & 0xFF;
    send_buf[9] = alt & 0xFF;
    /* 飞行模式 */
    send_buf[10] = fly_mode;
    /* 锁定状态 */
    send_buf[11] = armed;
    
    /* 状态帧的功能字为0x01 */
    NimingOldReportBuffer(0x01, send_buf, 12);
}

/**
 * @brief       发送传感器帧至匿名地面站V4
 * @param       acc_x : x轴上的加速度值
 *              acc_y : y轴上的加速度值
 *              acc_z : z轴上的加速度值
 *              gyro_x: x轴上的陀螺仪值
 *              gyro_y: y轴上的陀螺仪值
 *              gyro_z: z轴上的陀螺仪值
 *              mag_x : x轴上的磁力计值（ATK-MS6050不支持）
 *              mag_y : y轴上的磁力计值（ATK-MS6050不支持）
 *              mag_z : z轴上的磁力计值（ATK-MS6050不支持）
 * @retval      无
 */
void VisualOsc::NimingOldReportIMU(int16_t  acc_x, int16_t  acc_y, int16_t  acc_z,
                          int16_t gyro_x, int16_t gyro_y, int16_t gyro_z,
                          int16_t  mag_x, int16_t  mag_y, int16_t  mag_z)
{
    uint8_t send_buf[18];
    
    /* x轴上的加速度值 */
    send_buf[0] = (acc_x >> 8) & 0xFF;
    send_buf[1] = acc_x & 0xFF;
    /* y轴上的加速度值 */
    send_buf[2] = (acc_y >> 8) & 0xFF;
    send_buf[3] = acc_y & 0xFF;
    /* z轴上的加速度值 */
    send_buf[4] = (acc_z >> 8) & 0xFF;
    send_buf[5] = acc_z & 0xFF;
    /* x轴上的陀螺仪值 */
    send_buf[6] = (gyro_x >> 8) & 0xFF;
    send_buf[7] = gyro_x & 0xFF;
    /* y轴上的陀螺仪值 */
    send_buf[8] = (gyro_y >> 8) & 0xFF;
    send_buf[9] = gyro_y & 0xFF;
    /* z轴上的陀螺仪值 */
    send_buf[10] = (gyro_z >> 8) & 0xFF;
    send_buf[11] = gyro_z & 0xFF;
    /* x轴上的磁力计值 */
    send_buf[12] = (mag_x >> 8) & 0xFF;
    send_buf[13] = mag_x & 0xFF;
    /* y轴上的磁力计值 */
    send_buf[14] = (mag_y >> 8) & 0xFF;
    send_buf[15] = mag_y & 0xFF;
    /* z轴上的磁力计值 */
    send_buf[16] = (mag_z >> 8) & 0xFF;
    send_buf[17] = mag_z & 0xFF;
    
    /* 传感器的功能字为0x02 */
    NimingOldReportBuffer(0x02, send_buf, 18);
}

void VisualOsc::NimingSendCurveBuffer(int ch, uint8_t *buf, int len)
{
    if(ch >= 10)
        return;

    //0xF1-0XFA
    uint8_t cmd = 0xF1 + ch;
    uint8_t send_buf[64];
    int index = 0;

    send_buf[index++] = 0xAB;
    send_buf[index++] = 0x00;
    send_buf[index++] = 0x00;
    send_buf[index++] = cmd;
    send_buf[index++] = len & 0xff;
    send_buf[index++] = (len >> 8) & 0xff;

    for(int i = 0; i < len; i++)
    {
        send_buf[index++] = buf[i];
    }

    uint8_t sum = 0, addsum = 0;
    for (int i = 0; i < index; i++)     /* 计算校验和 */
    {
        sum += send_buf[i];
        addsum += sum;
    }
    send_buf[index++] = sum;
    send_buf[index++] = addsum;

    WriteToStream(send_buf, index);
}

void VisualOsc::ShangwaiSendCurve(uint8_t *buf, int len)
{
    ShangwaiSendHead();
    WriteToStream(buf, len);
    ShangwaiSendTail();
}

/// @brief 
/// @param line 
void VisualOsc::PlotterSendCurve(const std::string &line)
{
    WriteToStream((uint8_t*)&"$", 1);
    WriteToStream((uint8_t*)line.c_str(), line.size());
    WriteToStream((uint8_t*)&";", 1);
}

void VisualOsc::WriteToStream(const uint8_t *buf, int len)
{
    if(m_enabled)
        FW_UartSend(VISUAL_OSC_UART_PORT_ID, buf, len);
}

/**
 * @brief       通过串口1发送数据至匿名地面站V4
 * @param       cmd: 功能字
 *              dat: 待发送的数据（最多28字节）
 *              len: dat数据的有效位数
 * @retval      无
 */
void VisualOsc::NimingOldReportBuffer(uint8_t cmd, uint8_t *dat, uint8_t len)
{
    uint8_t send_buf[32];
    uint8_t i;
    
    if (len > 28)
    {
        return;
    }
    
    send_buf[len+4] = 0;            /* 校验位清零 */
    send_buf[0] = 0xAA;             /* 帧头为0xAAAA */
    send_buf[1] = 0xAA;             /* 帧头为0xAAAA */
    send_buf[2] = cmd;              /* 功能字 */
    send_buf[3] = len;              /* 数据长度 */
    for (i=0; i<len; i++)           /* 复制数据 */
    {
        send_buf[4 + i] = dat[i];
    }
    for (i=0; i<(len + 4); i++)     /* 计算校验和 */
    {
        send_buf[len + 4] += send_buf[i];
    }
    
    /* 发送数据 */
    WriteToStream(send_buf, len + 5);
}

void VisualOsc::ShangwaiSendHead()
{
    const uint8_t buf[2] = {0x03, 0xfc};
    
    WriteToStream(buf, 2);
}

void VisualOsc::ShangwaiSendTail()
{
    const uint8_t buf[2] = {0xfc, 0x03};
    
    WriteToStream(buf, 2);
}
